#include "PWMGenerater.h"
#include "BoardBase.h"
#include "MathUtil.h"
#include "PIDProvider.h"

#define LOG_TAG "Motor"

#include <elog.h>

/**
 * @brief 电机模式
 */
typedef uint8_t MotorMode;

#define Free ((MotorMode)0x00)
#define Brake ((MotorMode)0x01)
#define Forward ((MotorMode)0x02)
#define Backward ((MotorMode)0x03)

typedef struct Motor
{
    /**
     * @brief 电机驱动控制引脚
     */
    Pin_t IN1;
    Pin_t IN2;

    MotorMode mode;
    PWMGenerater_t pwm;

    /**
     * @brief 速度环
     */
    PIDProvider_t pid;
} Motor_t, *pMotor;

/**
 * @brief 设置电机正反转、停机、自由转动
 * 
 * @param motor 电机
 * @param mode 正反转、停机、自由转动
 */
void set_motor_mode(pMotor motor, MotorMode mode)
{
    motor->mode = mode;

    GPIO_TypeDef *port1 = motor->IN1.port;
    uint16_t pin1 = motor->IN1.pin;
    GPIO_TypeDef *port2 = motor->IN2.port;
    uint16_t pin2 = motor->IN2.pin;

    switch (mode)
    {
    case Free:
        HAL_GPIO_WritePin(port1, pin1, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(port2, pin2, GPIO_PIN_RESET);
        break;
    case Brake:
        HAL_GPIO_WritePin(port1, pin1, GPIO_PIN_SET);
        HAL_GPIO_WritePin(port2, pin2, GPIO_PIN_SET);
        break;
    case Forward:
        HAL_GPIO_WritePin(port1, pin1, GPIO_PIN_SET);
        HAL_GPIO_WritePin(port2, pin2, GPIO_PIN_RESET);
        break;
    case Backward:
        HAL_GPIO_WritePin(port1, pin1, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(port2, pin2, GPIO_PIN_SET);
        break;
    }
}

/**
 * @brief 根据转度误差修正电机转速
 * 
 * 认为：电机转速与其Duty值呈线性关系
 * 此处输入为速度的增量，自然PID输出的是Duty的增量
 * 
 * @param motor 电机
 * @param err 转速误差 - 正(Duty加)负(Duty减)
 */
void update_motor_speed(pMotor motor, float err)
{
    float deltaDuty = pid_push_new_err(&motor->pid, err);
    Duty curDuty = get_pwm_duty(motor->pwm);

    Duty newDuty;
    if (deltaDuty < 0)
    {
        newDuty = UnsignedUFSafe(curDuty, deltaDuty, 0);
        log_d("Delta<0:%f,curDuty:%d, newDuty:%d", deltaDuty, curDuty, newDuty);
    }
    else
    {
        newDuty = curDuty + deltaDuty;
        newDuty = min(newDuty, motor->pwm.maxDuty);
        log_d("Delta>0:%f,curDuty:%d, newDuty:%d", deltaDuty, curDuty, newDuty);
    }

    set_pwm_duty(motor->pwm, newDuty);
}
